#pragma once
#include "ecal/ecal.h"
#include "ecal/msg/protobuf/publisher.h"
#include "ecal/msg/protobuf/subscriber.h"
#include "xict_cpp/core/stage/base.h"
#include "xict_cpp/utils/util.h"
#include "xict_proto/sensor/can.pb.h"
#include "xict_proto/sensor/image.pb.h"

namespace xict_cpp {
    namespace core {
        namespace stage {

            struct RotationAngleSubscriber {
#ifdef GRADER
                /// @brief 平地机状态数据订阅器
                std::shared_ptr<
                    eCAL::protobuf::CSubscriber<xcmg_proto::Grader_CAN>>
                    rotation_angle_sub;
#endif

#ifdef LOADER
                /// @brief 装载机状态数据订阅器
                std::shared_ptr<
                    eCAL::protobuf::CSubscriber<xcmg_proto::Loader_CAN>>
                    rotation_angle_sub;
#endif

#ifdef BACKHOE_LOADER
                /// @brief 压路机状态数据订阅器
                std::shared_ptr<
                    eCAL::protobuf::CSubscriber<xcmg_proto::BackhoeLoader_CAN>>
                    rotation_angle_sub;
#endif

#ifdef SCRAPER
                /// @brief 铲运机状态数据订阅器
                std::shared_ptr<
                    eCAL::protobuf::CSubscriber<xcmg_proto::Scraper_CAN>>
                    rotation_angle_sub;
#endif
            };

            ///@brief 三维贝塞尔曲线绘制点
            struct BezierCurveParams {
                /// @brief 起点
                cv::Point3d start_point;
                /// @brief 控制点
                cv::Point3d control_point;
                /// @brief 终点
                cv::Point3d end_point;
            };

            struct ScriptsPath {
                /// @brief unity脚本开始执行路径
                std::string unity_start_script = "";
                /// @brief unity脚本结束执行路径
                std::string unity_end_script = "";
                /// qt界面执行路径
                std::string ui_app_path = "";
            };

            struct DrawTrajectoryStageParams {
                /// @brief 轨迹形式
                std::string trajectory_type = "MULTIGLOWARROWS";
                /// @brief 轨迹图像发布的topic
                std::string pub_topic = "";
                /// @brief 使能topic发布
                bool enable_pub = false;
                /// @brief 旋转向量
                cv::Vec3d rotation_vector =
                    cv::Vec3d(2.20647, 0.0541786, -0.0235578);
                /// @brief 平移向量
                cv::Vec3d translation_vector =
                    cv::Vec3d(-0.0162412, 0.11793, 0.481411);
                /// @brief 相机内参矩阵
                cv::Mat camera_matrix = cv::Mat::eye(3, 3, CV_64FC1);
                /// @brief 相机畸变参数矩阵
                cv::Mat dist_coeffs = cv::Mat::zeros(5, 1, CV_64FC1);
                /// @brief 图像宽度
                int image_width = 1920;
                /// @brief 图像高度
                int image_height = 1080;
                /// @brief 图像类型
                int image_type = CV_8UC3;
                /// @brief 拟合点数
                int num_points = 1000;

                /// @brief 线宽
                int line_thickness = 15;
                /// @brief 线颜色
                std::vector<int> line_color = {255, 0, 0};
                /// @brief 荧光宽度
                int glow_line_width = 3;
                /// @brief 荧光色渐变比率
                double glow_line_ratio = 0.2;
                /// @brief 荧光色线宽度
                int glow_line_thickness = 2;

                /// @brief 5m位置点
                int position_5m = 500;
                /// @brief 10m位置点
                int position_10m = 1000;
                /// @brief 15m位置点
                int position_15m = 1500;
                /// @brief 距离标识绘制高度x偏移
                int text_position_x = 0;
                /// @brief 距离标识绘制高度y偏移
                int text_position_y = 10;

                /// @brief 距离标识字体内颜色
                std::vector<int> font_color_inner = {0, 0, 0};
                /// @brief 距离标识字体外颜色
                std::vector<int> font_color_outer = {255, 255, 255};
                /// @brief 距离标识字体荧光宽度
                int glow_font_width = 3;
                /// @brief 距离标识字体大小
                double font_size = 0.6;
                /// @brief 距离标识字体粗细
                double font_thickness = 1.0;

                /// @brief 箭头流动速度
                double arrow_speed = 0.3;
                /// @brief 箭头粗细
                int arrow_thickness = 3;
                /// @brief 箭头长度
                int arrow_length = 20;
                /// @brief 箭头角度
                double arrow_angle = CV_PI / 6.0;
                /// @brief 箭头颜色
                std::vector<int> arrow_color = {0, 255, 0};
                /// @brief 箭头荧光宽度
                int glow_arrow_width = 3;
                /// @brief 箭头荧光比率
                double glow_arrow_ratio = 0.2;
                /// @brief 荧光色线宽度
                int glow_arrow_thickness = 2;
                /// @brief 多箭头绘制间距
                int arrow_interval = 250;

                /// @brief 定义10种颜色的map
                std::map<std::string, cv::Scalar> color_maps = {
                    {"red", cv::Scalar(0, 0, 255)},
                    {"green", cv::Scalar(0, 255, 0)},
                    {"blue", cv::Scalar(255, 0, 0)},
                    {"cyan", cv::Scalar(255, 255, 0)},
                    {"magenta", cv::Scalar(255, 0, 255)},
                    {"yellow", cv::Scalar(0, 255, 255)},
                    {"drange", cv::Scalar(0, 165, 255)},
                    {"purple", cv::Scalar(128, 0, 128)},
                    {"brown", cv::Scalar(42, 42, 165)},
                    {"pink", cv::Scalar(203, 192, 255)}};
                /// @brief 线条分段颜色
                std::vector<std::string> multi_colors = {"red", "green",
                                                         "blue"};
                /// @brief 线段过度比率
                double multi_color_alpha_ratio = 0.3;
                /// @brief 线段颜色透明度
                int alpha_channel_transparency = 200;

                /// @brief 待拟合的点
                std::vector<BezierCurveParams> curves_points;
                /// @brief 线长度
                double line_length = 0.5;
                /// @brief 车型
                std::string vehicle_type = "";
            };

            class DrawTrajectoryStage : public Stage {
            public:
                /// @brief 默认构造函数
                DrawTrajectoryStage() = default;
                /// @brief 析构函数
                ~DrawTrajectoryStage() override = default;

                /// @brief 初始化函数，初始化stage的状态和参数
                bool Init() override;
                /// @brief 带参数的初始化函数
                bool Init(const std::vector<DrawTrajectoryStageParams>& params);
                /// @brief 处理函数，处理输入数据
                bool Process() override;
                /// @brief 返回类的名称
                std::string Name() const override {
                    return "DrawTrajectoryStage";
                }
                /// @brief 发布图像ecal数据
                void PublishEcalImage(
                    const cv::Mat& img,
                    eCAL::protobuf::CPublisher<xcmg_proto::OpencvImage>& pub);

            protected:
                /// @brief 初始化函数，并返回结果
                bool Initialize() override;
                /// @brief 重置函数
                void Reset() override;

                /// @brief 绘制轨迹+单个流动箭头
                cv::Mat DrawTrajectoryWithSingleGlowArrow(
                    const DrawTrajectoryStageParams& params,
                    const std::vector<std::vector<cv::Point3d>>&
                        trajectory_points_list,
                    double time);
                /// @brief 绘制轨迹+多个流动箭头
                cv::Mat DrawTrajectoryWithMultipleGlowArrows(
                    const DrawTrajectoryStageParams& params,
                    const std::vector<std::vector<cv::Point3d>>&
                        trajectory_points_list,
                    double time);
                /// @brief 绘制轨迹+轨迹颜色流动
                cv::Mat DrawTrajectoryWithColorGlow(
                    const DrawTrajectoryStageParams& params,
                    const std::vector<std::vector<cv::Point3d>>&
                        trajectory_points_list,
                    double time);
                /// @breif 绘制轨迹+渐变色流动
                cv::Mat DrawTrajectoryWithGradientColorGlow(
                    const DrawTrajectoryStageParams& params,
                    const std::vector<std::vector<cv::Point3d>>&
                        trajectory_points_list,
                    double time);

                /// @brief 角度订阅回调函数
                template <typename T>
                void CallbackRotationAngle(const char* topic_name,
                                           const T& data, long long time,
                                           long long clock, long long id) {
#ifdef SCRAPER
                    angle_of_rotation_ = data.turn_angle();
#endif
                }

            private:
                /// @brief DrawlineStage的配置参数
                std::vector<DrawTrajectoryStageParams> params_lists_;
                /// @brief angle of rotation
                double angle_of_rotation_ = 0.0;
                /// @brief 颜色分段步长
                double color_step_ = 1.0;
                /// @brief 旋转角度订阅器
                RotationAngleSubscriber sub_;
                /// @brief线程锁
                std::mutex mt_;
                /// @brief 轨迹图像发布器
                std::vector<eCAL::protobuf::CPublisher<xcmg_proto::OpencvImage>>
                    pubs_;
            };
        }   // namespace stage
    }       // namespace core
}   // namespace xict_cpp